﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using SimpleJSON;
using ROSBridgeLib;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using UnityEngine.UI;



public class MapDisplayer : MonoBehaviour
{


    private ROSBridgeWebSocketConnection ros = null;

    private ROSBridgeSubscriber mapSubscriber;


    public RawImage mapImage;

    Texture2D texture;


    int[,] mapData;

    bool needUpdate;

    const int mapSize = 200;



    // Use this for initialization
    void Start()
    {

        texture = new Texture2D(mapSize, mapSize);

        mapData = new int[mapSize, mapSize];

        mapImage.material.mainTexture = texture;



        for (int x = 0; x < mapSize; x++)
        {
            for (int y = 0; y < mapSize; y++)
            {

                mapData[x, y] = -1;
              

            }


        }

		
    }
	
    // Update is called once per frame
    void Update()
    {
		



        if (needUpdate)
        {


            for (int x = 0; x < mapSize; x++)
            {
                for (int y = 0; y < mapSize; y++)
                {

                    int i = mapData[mapSize - 1 - x, y];
                 


                    if (i == 100)
                    {
                        texture.SetPixel(x, y, Color.black);

                    }

                    if (i == 0)
                    {
                        texture.SetPixel(x, y, Color.gray);

                    }


                }


            }

            texture.Apply();

            needUpdate = false;



        }

     
        if (ros != null)
        {
            ros.Render();

        }
    }

    public void Connect()
    {

        ros = new ROSBridgeWebSocketConnection("ws://192.168.1.38", 9090);
       
        ros.AddSubscriber(typeof(MapSuscribler));

        ros.Connect();


    }



    public void CheckAndRefresh()
    {

     
    }

    //    private void OnMessageArrived(JObject node)
    //    {
    //
    //
    //
    //        long startTime= System.DateTime.Now.Ticks;
    //
    //
    //
    //        JObject map=node;
    //
    //
    //        Debug.Log("Parser cost:"+((System.DateTime.Now.Ticks-startTime)/10000)+"ms");
    //
    //        startTime= System.DateTime.Now.Ticks;
    //
    //        int  width =(int)map["msg"]["info"]["width"];
    //        int  height=(int)map["msg"]["info"]["height"];
    //
    //
    //
    //        Debug.Log("GET map Size:"+width+"x"+height);
    //
    //
    //
    //        JArray a2=(JArray)map["msg"]["data"];
    //
    //        {
    //
    //
    //            Debug.Log("Lenth:"+a2.Count);
    //
    //
    //            int x,y;
    //
    //            for (int i=0;i<a2.Count;i++)
    //            {
    //
    //                int p;
    //
    //                x=i / height;
    //                y=i % height;
    //
    //
    //                p=(int)a2[i];
    //
    //                mapData[x,y]=p;
    //
    //
    //            }
    //
    //
    //            needUpdate=true;
    //
    //
    //            Debug.Log("FillArray cost:"+((System.DateTime.Now.Ticks-startTime)/10000)+"ms");
    //
    //
    //        }
    //
    //
    //
    //    }


    void OnApplicationQuit()
    {

        if (ros != null)
        {
            ros.Disconnect();
        }
    }



}
